/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package src;

import util.*;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends SimpleRobot {
    // Contol Parameters
    public static final int CONTROL_RAW = 0;
    public static final int CONTROL_PID = 1;

    // PID Constants
    public static final double DRIVE_TRAIN_ENCODER_FACTOR = 0.00436;
    public static final double DRIVE_TRAIN_PID_P = 0.09;
    public static final double DRIVE_TRAIN_PID_I = 0.0275;
    public static final double DRIVE_TRAIN_PID_D = -0.013;
    public static final double DRIVE_TRAIN_PID_F = 0;

    public static final double PAYLOAD_SHOOTER_ENCODER_FACTOR = 1.25;
    public static final double PAYLOAD_SHOOTER_PID_MAX = 4000;
    public static final double PAYLOAD_SHOOTER_PID_STEP = 20;
    public static final double PAYLOAD_SHOOTER_PID_P = 0.0005;
    public static final double PAYLOAD_SHOOTER_PID_I = 0.00012;
    public static final double PAYLOAD_SHOOTER_PID_D = -0.0001;
    public static final double PAYLOAD_SHOOTER_PID_F = 0;

    // Camera Tracking Constants
    public static final double CAMERA_HORIZONTAL_FOV = 17.5;
    public static final double CAMERA_TURNING_FACTOR = 0.5;

    // General Hardware
    public static CommandReceiver commandReceiver;
    public static Joystick joystick1, joystick2, joystick3, joystick4;
    public static AimingCalculator aimingCalculator;
    public static Compressor compressor;

    // Drive Train Hardware
    public static SpeedController driveTrainLeftMotors, driveTrainRightMotors;
    public static RobotDrive driveTrainRobotDrive;
    public static RateEncoder driveTrainLeftEncoder, driveTrainRightEncoder;
    public static PIDController driveTrainLeftPID, driveTrainRightPID;
    public static Solenoid driveTrainLowShifter, driveTrainHighShifter;

    // Payload Hardware
    public static SpeedController payloadShooterMotors;
    public static RateEncoder payloadShooterEncoder;
    public static PIDController payloadShooterPID;
    public static Solenoid payloadShooterActuatorOff, payloadShooterActuatorOn, payloadNetActuatorOff, payloadNetActuatorOn;

    // Systems
    public static DriveTrain driveTrain;
    public static Payload payload;

    public Robot() {
        commandReceiver = new Receiver();

        joystick1 = new Joystick(1);
        joystick2 = new Joystick(2);
        joystick3 = new Joystick(3);
        joystick4 = new Joystick(4);

        compressor = new Compressor(1, 1);
        compressor.start();

        driveTrainLeftMotors = new DoubleVictor(1, 1, 1, 2);
        driveTrainRightMotors = new DoubleVictor(1, 3, 1, 4);

        driveTrainRobotDrive = new RobotDrive(driveTrainLeftMotors, driveTrainRightMotors);
        driveTrainRobotDrive.setSafetyEnabled(false);

        driveTrainLeftEncoder = new RateEncoder(1, 2, 1, 3);
        driveTrainLeftEncoder.setReverseDirection(true);
        driveTrainLeftEncoder.setDistancePerPulse(DRIVE_TRAIN_ENCODER_FACTOR);
        driveTrainLeftEncoder.start();

        driveTrainRightEncoder = new RateEncoder(1, 4, 1, 5);
        driveTrainRightEncoder.setReverseDirection(true);
        driveTrainRightEncoder.setDistancePerPulse(DRIVE_TRAIN_ENCODER_FACTOR);
        driveTrainRightEncoder.start();

        driveTrainLeftPID = new PIDController(DRIVE_TRAIN_PID_P, DRIVE_TRAIN_PID_I, DRIVE_TRAIN_PID_D, DRIVE_TRAIN_PID_F,
                driveTrainLeftEncoder, driveTrainLeftMotors);
        driveTrainLeftPID.setInputRange(-1, 1);
        driveTrainLeftPID.setOutputRange(-1f, 1f);
        driveTrainLeftPID.disable();

        driveTrainRightPID = new PIDController(DRIVE_TRAIN_PID_P, DRIVE_TRAIN_PID_I, DRIVE_TRAIN_PID_D, DRIVE_TRAIN_PID_F,
                driveTrainRightEncoder, driveTrainRightMotors);
        driveTrainRightPID.setInputRange(-1, 1);
        driveTrainRightPID.setOutputRange(-1f, 1f);
        driveTrainRightPID.disable();

        driveTrainLowShifter = new Solenoid(1, 1);
        driveTrainHighShifter = new Solenoid(1, 2);

        payloadShooterMotors = new DoubleVictor(1, 5, 1, 6);

        payloadShooterEncoder = new RateEncoder(1, 6, 1, 7);
        payloadShooterEncoder.setReverseDirection(true);
        payloadShooterEncoder.setDistancePerPulse(PAYLOAD_SHOOTER_ENCODER_FACTOR);
        payloadShooterEncoder.start();

        payloadShooterPID = new PIDController(PAYLOAD_SHOOTER_PID_P, PAYLOAD_SHOOTER_PID_I, PAYLOAD_SHOOTER_PID_D, PAYLOAD_SHOOTER_PID_F,
                payloadShooterEncoder, payloadShooterMotors);
        payloadShooterPID.setInputRange(0, PAYLOAD_SHOOTER_PID_MAX);
        payloadShooterPID.setOutputRange(0, 1);
        payloadShooterPID.disable();

        payloadShooterActuatorOff = new Solenoid(1, 8);
        payloadShooterActuatorOn = new Solenoid(1, 5);
        payloadNetActuatorOff = new Solenoid(1, 4);
        payloadNetActuatorOn = new Solenoid(1, 3);

        driveTrain = new DriveTrain();
        payload = new Payload();

        driveTrain.setMode(CONTROL_RAW);
        payload.setShooterMode(CONTROL_RAW);
    }

    private void autoFire() {
        payload.setFire(true);

        try {
            Thread.sleep(2000);
        } catch (InterruptedException ex) {
            ex.printStackTrace();
        }

        payload.setFire(false);

        try {
            Thread.sleep(1500);
        } catch (InterruptedException ex) {
            ex.printStackTrace();
        }
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        payload.setShooter(-1.0);

        try {
            Thread.sleep(3500);
        } catch (InterruptedException ex) {
            ex.printStackTrace();
        }

        autoFire();
        autoFire();
        payload.setNet(true);
        try {
            Thread.sleep(1500);
        } catch (InterruptedException ex) {
            ex.printStackTrace();
        }
        while(DriverStation.getInstance().isAutonomous()) {
            autoFire();
        }
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        while(DriverStation.getInstance().isOperatorControl()) {
            traditionalControl();

            try {
                Thread.sleep(20);
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }
    }
    
    /**
     * This function is called once each time the robot enters test mode.
     */
    public void test() {}

    private void commandBasedUpdate() {
        while(commandReceiver.isCommandAvailable()) {
            CommandReceiver.Command command = commandReceiver.pollCommand();

            // SetDriveControlMode 1 s
            if(command.getName().equalsIgnoreCase("SetDriveControlMode")) {
                if(command.getArgument(0).equalsIgnoreCase("0")) {
                    driveTrain.setMode(CONTROL_RAW);
                } else if(command.getArgument(0).equalsIgnoreCase("1")) {
                    driveTrain.setMode(CONTROL_PID);
                }
            }

            // SetDriveSpeeds 2 d d
            if(command.getName().equalsIgnoreCase("SetDriveSpeeds")) {
                double left = Double.parseDouble(command.getArgument(0));
                double right = Double.parseDouble(command.getArgument(1));
                driveTrain.set(left, right);
            }

            // ResetDrivePID 0
            if(command.getName().equalsIgnoreCase("ResetDrivePID")) {
                driveTrain.resetPID();
            }

            // SetGear 1 s
            if(command.getName().equalsIgnoreCase("SetGear")) {
                if(command.getArgument(0).equalsIgnoreCase("0")) {
                    driveTrain.shiftLow();
                } else if(command.getArgument(0).equalsIgnoreCase("1")) {
                    driveTrain.shiftHigh();
                }
            }

            // SetShooterSpeedControlMode 1 s
            if(command.getName().equalsIgnoreCase("SetShooterSpeedControlMode")) {
                if(command.getArgument(0).equalsIgnoreCase("0")) {
                    payload.setShooterMode(CONTROL_RAW);
                } else if (command.getArgument(0).equalsIgnoreCase("1")) {
                    payload.setShooterMode(CONTROL_PID);
                }
            }

            // SetShooterSpeed 1 d
            if(command.getName().equalsIgnoreCase("SetShooterSpeed")) {
                double value = Double.parseDouble(command.getArgument(0));
                payload.setShooter(value);
            }

            // ResetShooterSpeedPID 0
            if(command.getName().equalsIgnoreCase("ResetShooterSpeedPID")) {
                payload.resetShooterPID();
            }

            // SetShooterOrientation 1 s
            if(command.getName().equalsIgnoreCase("SetShooterOrientation")) {
            }

            // UseCameraTracking 0
            if(command.getName().equalsIgnoreCase("UseCameraTracking")) {
                driveTrain.shiftLow();
                double xError = (SmartDashboard.getNumber("X Error", 0.5) - 0.5) * CAMERA_TURNING_FACTOR;
                driveTrain.set(xError, -xError);
                payload.setShooter(PAYLOAD_SHOOTER_PID_MAX);
            }

            // FireFrisbee 0
//            if(command.getName().equalsIgnoreCase("FireFrisbee")) {
//                payload.fire();
//            }

            // OverrideLimitSwitch 1 s
            if(command.getName().equalsIgnoreCase("OverrideLimitSwitch")) {
            }
        }
    }

    private void traditionalControl() {
        if(joystick2.getRawButton(2)) {
            driveTrain.shiftLow();
        } else if(joystick2.getRawButton(3)) {
            driveTrain.shiftHigh();
        }

        driveTrain.set(joystick1.getY(), joystick2.getY());

        payload.setFire(joystick4.getRawButton(1));
        if(joystick4.getRawButton(2)) payload.setNet(false);
        if(joystick4.getRawButton(3)) payload.setNet(true);

//        double shooterValue = payload.getShooter() - joystick4.getY() * PAYLOAD_SHOOTER_PID_STEP;
//        if(shooterValue > PAYLOAD_SHOOTER_PID_MAX) shooterValue = PAYLOAD_SHOOTER_PID_MAX;
//        if(shooterValue < 0.0) shooterValue = 0.0;
//        payload.setShooter(shooterValue);

        payload.setShooter(Math.min(0.0, joystick4.getY()));
    }

    private void checkCalibrationFiles() {
        System.out.println("Aiming Test (Statistic): ");
        System.out.println("==================");

        double targetDistance = 8.5; //meters
        double targetAltitude = 2.5; //meters
        boolean frisbeeFlipped = false;

        double shooterElevation= aimingCalculator.getShooterElevation(targetDistance, targetAltitude, frisbeeFlipped); //checks everything installed

        double frisbeeAzimuth= aimingCalculator.getFrisbeeAzimuth(targetDistance, shooterElevation, frisbeeFlipped); // checks eerything installed

        double frisbeeAltitude= aimingCalculator.getFrisbeeAltitude(targetDistance, shooterElevation, frisbeeFlipped); // checks everything installed

        System.out.println("Final Analysis: ");
        System.out.println("Target Distance   = "+ targetDistance+" meters");
        System.out.println("Target Altitude   = "+ targetAltitude+" meters");
        System.out.println("Frisbee Flipped   = "+ frisbeeFlipped);
        System.out.println("Shooter Elevation = "+ shooterElevation+" degrees");
        System.out.println("Frisbee Azimuth   = "+ frisbeeAzimuth+" degrees");
        System.out.println("Frisbee Altitude  = "+ frisbeeAltitude+" meters");

        System.out.println("=======================================");
    }
}
